function [PX,PY,PZ,seeds]=foregroundSeg(imD,imX,imY,imA,imC,thre)
%%%
% ForegroundSeg implements foreground segmentation 
%   
% The first seed is the point with the highest confidence and
% shorest distant. Then seed points are expanded while the 
% distance is less than a threshold(thre) unil no more seed
% is added. 
%
% Hu Ninghang 2010

    debug = false;
    seeds = find(imD == min(imD(imC == max(max(imC)))));
    expansion = find(imD == min(imD(imC == max(max(imC)))));
    
    % loop until convergence
    while(size(expansion,2) ~= 0)
        temp = [];
        for i = 1:size(expansion,2)
            cand = [expansion(i)-144,expansion(i)-1,...
                expansion(i)+1,expansion(i)+144];
            cand = cand(cand > 0 & cand < 25344);
            cand = setdiff(cand,seeds);
            if size(cand,2) ~= 0
                s = [imX(expansion(i));imY(expansion(i));imD(expansion(i))];
                c = [imX(cand);imY(cand);imD(cand)];
                d = sqrt(sum((c-repmat(s,1,size(c,2))).^2));
                temp = [temp,cand(d<thre)];
            end
        end
        if size(temp,2) == 0
            break
        end
        temp = unique(temp);
        expansion = temp;
        seeds = [seeds;expansion(:)];
    end

    mask = seeds;
    PX = imX(mask);
    PY = imY(mask);
    PZ = imD(mask);
    
    if debug
        % show and plot
        showD = imD;
        showD(seeds) = 0;
        subplot(2,3,1);
        imshow(showD,[]);
        subplot(2,3,2);
        imshow(imA,[]);
        subplot(2,3,3);
        imshow(imC,[]);
        colormap(jet);
        subplot(2,3,4:6);

        plot3(PX(:),PY(:),PZ(:),'.');
        set(gca,'ZDir','reverse')
        axis equal
        drawnow;
    end
end